#ifndef __STD_UTILS_NURBS__
#define __STD_UTILS_NURBS__

#include <vector>
#include <map>
#include <sstream>
#include <string>
#include <list>
#include "mesh.h"
#include "node.h"

template<class type>
size_t GetIndexOld(const math::Vec<type, 3>& P, std::map<std::string, size_t>& mapPosition, std::vector< math::Vec<type, 3> >& position) {
	typedef math::Vec<type, 3> Vec3n;
	std::stringstream ss;

	ss << std::fixed;
	Vec3n P2 = math::V3n(P[0], P[1], P[2]);

	char buf[256];
	sprintf(buf, "%.3f, %.3f, %.3f", P2[0], P2[1], P2[2]);
	ss << buf;

	if(mapPosition[ss.str()] == 0) {
		size_t i = position.size() + 1;
		mapPosition[ss.str()] = i;
		position.push_back(P);
		return i - 1;
	}
	
	return mapPosition[ss.str()] - 1;	
}

template<class type>
size_t GetIndex(const math::Vec<type, 3>& P, std::map<std::string, size_t>& mapPosition, std::vector< math::Vec<type, 3> >& position, const type eps = 0.001) {
	typedef math::Vec<type, 3> Vec3n;
	std::stringstream ss;

	ss << std::fixed;
	Vec3n P2 = math::V3n(P[0], P[1], P[2]);
	for(size_t i = 0; i < 3; i++) if(std::abs(P2[i]) < eps) P2[i] = 0.0;
	char buf[256];
	sprintf(buf, "%.3f, %.3f, %.3f", P2[0], P2[1], P2[2]);
	ss << buf;

	if(mapPosition[ss.str()] == 0) {
		size_t i = position.size() + 1;
		mapPosition[ss.str()] = i;
		position.push_back(P);
		return i - 1;
	}
	
	return mapPosition[ss.str()] - 1;	
}

template<class type>
std::pair< std::vector< math::Vec<type, 3> >, std::vector<size_t> > MergeTriangleMeshes(const std::vector< std::pair< std::vector< math::Vec<type, 3> >, std::vector<size_t> > >& tm) {
	typedef math::Vec<type, 3> Vec3n;
	std::map<std::string, size_t> mapPoint;
	std::vector<Vec3n> listPoint;
	std::vector<size_t> listIndex;

	for(size_t i = 0; i < tm.size(); i++) {
		const std::vector<Vec3n> vertex = tm[i].first;
		const std::vector<size_t> index = tm[i].second;

		for(size_t j = 0; j < index.size(); j += 3) {
			const Vec3n& p0 = vertex[index[j + 0]];
			const Vec3n& p1 = vertex[index[j + 1]];
			const Vec3n& p2 = vertex[index[j + 2]];

 			size_t a = GetIndexOld<type>(p0, mapPoint, listPoint);
 			size_t b = GetIndexOld<type>(p1, mapPoint, listPoint);
 			size_t c = GetIndexOld<type>(p2, mapPoint, listPoint);
 			listIndex.push_back(a - 1);
 			listIndex.push_back(b - 1);
 			listIndex.push_back(c - 1);
		}
	}

	return std::pair< std::vector<Vec3n>, std::vector<size_t> >(listPoint, listIndex);
}

template<class type>
std::pair< std::vector< math::Vec<type, 3> >, std::vector<math::Vec3ui> > MergeTriangleMeshes(const std::vector< std::pair< std::vector< math::Vec<type, 3> >, std::vector<math::Vec3ui> > >& tm) {
	typedef math::Vec<type, 3> Vec3n;
	std::map<std::string, size_t> mapPosition;
	std::vector<Vec3n> position;
	std::vector<math::Vec3ui> index;

	for(size_t i = 0; i < tm.size(); i++) {
		const std::vector<Vec3n>& positioni = tm[i].first;
		const std::vector<math::Vec3ui>& indexi = tm[i].second;
		for(size_t j = 0; j < indexi.size(); j++) {
			const math::Vec3ui k = indexi[j];

			size_t a = GetIndex<type>(positioni[k[0]], mapPosition, position);
			size_t b = GetIndex<type>(positioni[k[1]], mapPosition, position);
			size_t c = GetIndex<type>(positioni[k[2]], mapPosition, position);

			index.push_back(math::V3ui(a, b, c));
		}
	}

	return std::pair< std::vector<Vec3n>, std::vector<math::Vec3ui> >(position, index);

}

template<class type>
std::pair< std::vector< math::Vec<type, 3> >, std::vector<math::Vec3ui> > MergeDisjointTriangleMeshes(const std::vector< std::pair< std::vector< math::Vec<type, 3> >, std::vector<math::Vec3ui> > >& tm)  {
	typedef math::Vec<type, 3> Vec3n;
	size_t offset = 0;
	std::vector<Vec3n> position;
	std::vector<math::Vec3ui> index;
	for(size_t i = 0; i < tm.size(); i++) {
		const std::vector<Vec3n>& positioni = tm[i].first;
		const std::vector<math::Vec3ui>& indexi = tm[i].second;
		for(size_t j = 0; j < positioni.size(); j++)
			position.push_back(positioni[j]);
		for(size_t j = 0; j < indexi.size(); j++) {
			index.push_back(indexi[j] + math::V3ui(offset, offset, offset));
		}

		offset += positioni.size();
	}

	return std::pair< std::vector<Vec3n>, std::vector<math::Vec3ui> >(position, index);
}

std::vector<size_t> Intersection(const std::vector<size_t>& a, const std::vector<size_t>& b);
std::vector<size_t> Union(const std::vector<size_t>& a, const std::vector<size_t>& b);
std::vector<size_t> Difference(const std::vector<size_t>& a, const std::vector<size_t>& b);

template<class type>
void gauleg(type x1, type x2, std::vector< std::pair<type, type> >& s, size_t n, type EPS = 0.00001)
	  //Given the lower and upper limits of integration x1 and x2, and
	  //given n, this routine returns arrays x[1..n] and w[1..n] of
	  //length n, containing the abscissas and weights of the
	  //Gauss-Legendre n-point quadrature formula.
{
	s.resize(n);
	int m,j,i;

	type z1,z,xm,xl,pp,p3,p2,p1;
	//High precision is a good idea for this routine. The roots are
	//symmetric in the interval, so we only have to find half of them.
	 
	m=((int)n+1)/2;                                    
	xm=0.5*(x2+x1);                                    
	xl=0.5*(x2-x1);
	for (i=1;i<=m;i++)
	{
		//Loop over the desired roots.
		z=cos(3.141592654*(i-0.25)/(n+0.5));
		//Starting with the above approximation to the ith root, we
		//enter the main loop of refinement by Newton's method.
		do {
			p1=1.0;
			p2=0.0;
			for(j=1;j<=(int)n;j++)
			{
				// Loop up the recurrence relation to get the Legendre polynomial evaluated at z.
				p3=p2;                               
				p2=p1;
				p1=((2.0*j-1.0)*z*p2-(j-1.0)*p3)/j;
			}
			// p1 is now the desired Legendre polynomial. We next compute
			// pp, its derivative, by a standard relation involving also
			// p2, the polynomial of one lower order.
			pp=n*(z*p1-p2)/(z*z-1.0);
			z1=z;
			z=z1-p1/pp;                          //Newton's method.
		} while (fabs(z-z1) > EPS);
		// Scale the root to the desired interval, and put in its
		// symmetric counterpart.x
		s[i - 1].first = xm-xl*z;                             
		s[n+1-i - 1].first = xm+xl*z;
		
		//Compute the weight and its symmetric counterpart.
		s[i - 1].second =2.0*xl/((1.0-z*z)*pp*pp);            
		s[n+1-i - 1].second = s[i - 1].second;                            
	}
}

template<class V>
void Invert(std::pair< std::vector<V>, std::vector<size_t> >& tm) {
	for(size_t i = 0; i < tm.second.size(); i += 3) {
		size_t a = tm.second[i + 0];
		size_t b = tm.second[i + 1];
		size_t c = tm.second[i + 2];

		tm.second[i + 1] = c;
		tm.second[i + 2] = b;
	}
}

template<class type>
type Random(type a, type b) {
	if(b < a) {
		std::cerr << "error: a has to be less than b." << std::endl;
		return (type)0;
	}

	type r = (type)rand() / RAND_MAX;

	return a + r * (b - a);
}

template<class type, size_t dim, size_t width> 
math::Vec<type, dim> Center(math::Vec<type, dim> c[width][width][width]) {
	math::Vec<type, dim> center;
	for(size_t i = 0; i < width; i++) {
		for(size_t j = 0; j < width; j++) {
			for(size_t k = 0; k < width; k++) {
				center += c[i][j][k];
			}
		}
	}
	return (1.0 / (width * width * width)) * center;
}

template<class type, size_t dim> 
math::Vec<type, dim> Center(const std::vector< math::Vec<type, dim> >& c) {
	math::Vec<type, dim> center;
	for(size_t i = 0; i < c.size(); i++) {
		center += c[i];
	}
	return (1.0 / (c.size())) * center;
}


template<class type, size_t dim> 
math::Vec<type, dim> Average(const std::vector< math::Vec<type, dim> >& c) {
	math::Vec<type, dim> average;
	for(size_t i = 0; i < c.size(); i++) {
		average += c[i];
	}
	return ((type)1 / c.size()) * average;
}

template<class type, size_t dim, size_t width> 
void Transform(const math::Mat4x4<type>& T, math::Vec<type, dim> c[width][width][width]) {
	for(size_t i = 0; i < width; i++) {
		for(size_t j = 0; j < width; j++) {
			for(size_t k = 0; k < width; k++) {
				Vec4n x = T * math::V4n(c[i][j][k][0], c[i][j][k][1], c[i][j][k][2], 1.0);
				c[i][j][k][0] = x[0];
				c[i][j][k][1] = x[1];
				c[i][j][k][2] = x[2];
			}
		}
	}
}

template<class type>
void Transform(std::vector< math::Vec<nfloat, 3> >& position, const math::Mat4x4<type>& T) {
	for(size_t i = 0; i < position.size(); i++) {
		math::Vec<type, 3>& x = position[i];
		const math::Vec<type, 4> y = T * math::V4n(x, 1.0);
		x = math::V3n(y[0], y[1], y[2]);
	}
}


#endif //__STD_UTILS__
